#define THRESHOLD	4

//
//  main.cpp
//  SMCABC
//
//  Created by Paul Marjoram on 10/23/12.
//  Copyright (c) 2012 Paul Marjoram. All rights reserved.
//

using namespace std;

//#include "tchar.h"
#include<iostream>
#include <iomanip>
//#include<sstream>
#include<string>
#include<fstream>
#include<math.h>
#include <cassert>
#include <time.h>

#include <stdio.h>
#include <stdlib.h>
//#include <gsl/gsl_randist.h>
#include "../flysim.h"

using namespace std;

//ofstream ofile("SMCout1.txt",ios::out);
//const int ciNoOfParameters=3;
//float dParameterLowerBounds[ciNoOfParameters]={0,0,0};
//float dParameterUpperBounds[ciNoOfParameters]={10,10,10};
//const int ciDimensionOfData=3;
//const int ciNoOfAcceptancesPerGeneration=1000;
//int iNumberOfGenerationsForSMCAnalysis=20;
//float ThisGensParams[ciNoOfAcceptancesPerGeneration][ciNoOfParameters];
//int TempArray[ciNoOfParameters];
//bool bDoPureRejection=0; // generally this will be set to 0, but if we set it to one it will do rejection without SMC ( y setting the number of generations equal to 1
//
//
//
//// Some useful function declarations follow
//
//long int ranseed= -153425;
//
////*********************************************
///*  Random number generator from Numerical recipes (Ed 2, ran1) */
//#define IA 16807
//#define IM 2147483647
//#define AM (1.0/IM)
//#define IQ 127773
//#define IR 2836
//#define NTAB 32
//#define NDIV (1+(IM-1)/NTAB)
//#define EPS 1.2e-7
//#define RNMX (1.0-EPS)
//float rand01(long *idum)
//{
//	int j;
//	long k;
//	static long iy=0;
//	static long iv[NTAB];
//	float temp;
//	if(*idum<=0 || !iy) {
//		if(-(*idum) < 1) *idum=1;
//        else *idum= -(*idum);
//		for(j=NTAB+7;j>=0;j--) {
//			k=(*idum)/IQ;
//			*idum=IA*(*idum-k*IQ) -IR*k;
//			if(*idum < 0) *idum += IM;
//			if(j <NTAB) iv[j]= *idum;
//		}
//		iy=iv[0];
//	}
//	k=(*idum)/IQ;
//	*idum=IA*(*idum-k*IQ)-IR*k;
//	if(*idum < 0) *idum += IM;
//	j=iy/NDIV;
//	iy=iv[j];
//	iv[j]= *idum;
//	if((temp=AM*iy) > RNMX) return RNMX;
//	else return temp;
//}
//#ifdef TRAND
//main() {
//	int i,bin[10];
//	long dummy= -9284322;
//	for(i=0;i<10;i++) bin[i]=0;
//	for(i=0;i<20000;i++)
//		bin[(int)10*rand01(&dummy)]++;
//	for(i=0;i<10;i++)
//		printf("\n\t%d %lf",i,bin[i]/20000.0);
//}
//#endif
////**********************************************
//
//
#define SQRT_2 sqrt(2.0)
#define SQRT_PI sqrt(3.14159265)
//
///*Generator of normally distributed random variable from Numerical recipes (Ed 2, gasdev) */
//float gasdev(long *idum)
//{
//	static int iset=0;
//	static float gset;
//	float fac,rsq,v1,v2;
//	if (iset==0) {
//		do{
//			v1=2.0*rand01(&ranseed)-1.0;
//			v2=2.0*rand01(&ranseed)-1.0;
//			rsq=v1*v1+v2*v2;
//		}while(rsq>=1.0 ||rsq==0.0);
//		fac=sqrt(-2.0*log(rsq)/rsq);
//		gset=v1*fac;
//		iset=1;
//		return v2*fac;
//	}else{
//		iset=0;
//		return gset;
//	}
//}
//
//float NormalRV(float Mean, float StdDev)
//{
//    return Mean+StdDev*gasdev(&ranseed);
//}
//
////*******************************************************
//float erfcc(float x)
//{
//	float t,z,ans;
//	
//	z=fabs(x);
//	t=1.0/(1.0+0.5*z);
//	ans=t*exp(-z*z-1.26551223+t*(1.00002368+t*(0.37409196+t*(0.09678418+
//                                                             t*(-0.18628806+t*(0.27886807+t*(-1.13520398+t*(1.48851587+
//                                                                                                            t*(-0.82215223+t*0.17087277)))))))));
//	return x >= 0.0 ? ans : 2.0-ans;
//}
//
////********************************************************
//
//float P_Normal_greater_than_x(float xx, float mean, float sigma)
//{
//	// returns normalising constants for normal random variables
//	xx=xx-mean;
//	xx=xx/sigma;
//	return erfcc(xx/SQRT_2)/2.0;
//}
//
////********************************************************
//
float NormalDensity(float x, float mean, float StdDev)
{
	float dDen;
	dDen=1.0/(StdDev*SQRT_2*SQRT_PI);
	dDen=dDen*exp(-1*(x-mean)*(x-mean)/(2*StdDev*StdDev));
	return dDen;
}
//
////*******************************************************
//
//float ExponentialRV(float dLambda) // generates exponential random variables
//{
//	float p,x;
//	p=rand01(&ranseed);
//	x=-1*log(p)/dLambda;
//	return x;
//}
//
////*******************************************************
////*******************************************************
////********classe implemetations follow*******************
////*******************************************************
////*******************************************************
//
//
SMC::~SMC(){
}


/*float SMC::NormalRV(float Mean, float StdDev)
{

    //return Mean+StdDev*gasdev(&ranseed);
        return Mean+gsl_ran_gaussian(rg,StdDev);
}*/


SMC::SMC(){

rg = gsl_rng_alloc(gsl_rng_mt19937);
mpi_rank=MPI::COMM_WORLD.Get_rank();
mpi_ranks=MPI::COMM_WORLD.Get_size();


        gsl_rng_set(rg,time(0)+mpi_rank);

	assert(ciNoOfAcceptancesPerGeneration%mpi_ranks==0 );

    iNumberOfGenerationsForSMCAnalysis=100; // set this high for now..
    iMaxNumberOfAttempts=160000000;
    dRejectionThreshold=500;//12;
    StdDev=0.10;;//5;//0.5;
#define BOUND 5
if (mpi_rank==0)
cerr<<"True parameter values are: ";
for (int i=0;i<ciDimensionOfData;i++) {
        if (mpi_rank==0) {
        TestParamValues[i]=gsl_ran_flat(rg,0,10); 
        cerr<<"\t"<<TestParamValues[i];
        dParameterLowerBounds[i]=TestParamValues[i]-BOUND;
        dParameterUpperBounds[i]=TestParamValues[i]+BOUND;
        }
        MPI::COMM_WORLD.Bcast(&TestParamValues[i],1,MPI::DOUBLE,0);
        MPI::COMM_WORLD.Bcast(&dParameterLowerBounds[i],1,MPI::FLOAT,0);
        MPI::COMM_WORLD.Bcast(&dParameterUpperBounds[i],1,MPI::FLOAT,0);
}
if (mpi_rank==0)
cerr<<endl;
    ciNoOfAcceptsNeeded=ciNoOfAcceptancesPerGeneration;

}

void SMC::PrintWeightedParameterValues() {
float weighted_parameters[ciNoOfParameters];
for (int i=0;i<ciNoOfParameters;i++)  {
        weighted_parameters[i]=0;
        for (int j=0;j<ciNoOfAcceptancesPerGeneration;j++)
                weighted_parameters[i]+=NewAcceptedParameterValues[j][i]*dWeightsOfNewParametersValues[j];
        if (i==0)
                 fprintf(stderr,"%f",weighted_parameters[i]);
        else
                fprintf(stderr,"\t%f",weighted_parameters[i]);
}
cerr<<"\td="<<MeasureDistanceBetweenDataAndParameters()<<endl;
}


void SMC::PrintParameterValues(bool bPrintToFile)
{
    if (bPrintToFile==0)
    {
        std::cout<<"\n*************Parameter values follow***********";
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            std::cout<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                std::cout<<NewAcceptedParameterValues[i][j]<<" ";
            }
            std::cout<<"     weight: "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
        std::cout<<"\n***********************************************";
    }
    else
    {
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            ofile<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                ofile<<NewAcceptedParameterValues[i][j]<<" ";
            }
            ofile<<"      "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
    }
}


void SMC::BuildTestData()
{
    for (int i=0; i<ciNoOfTestDatasets; i++)
        for (int j=0;j<ciNoOfParameters;j++) {
                TestData[i][j]=TestParamValues[j]+gsl_ran_gaussian(rg,StdDev);//NormalRV(TestParamValues[j], StdDev);
        }
}

void SMC::PrintTestData()
{

float *summary=(float*)calloc(ciDimensionOfData,sizeof(float));
    std::cerr<<"*************Test data follows***********";
    for ( int i = 0; i < ciNoOfTestDatasets; ++i ) {
        std::cerr<<endl;
        for ( int j=0; j< ciDimensionOfData; j++) {
                summary[j]+=TestData[i][j];
            std::cerr<<TestData[i][j]<<"  ";
        }
    }
    std::cerr<<"\n*****************************************\n";
        for ( int j=0; j< ciDimensionOfData; j++) {
                cerr<<summary[j]/ciNoOfTestDatasets<<" ";
        }

    std::cerr<<"\n*****************************************\n";

:q}




void  SMC::RunSMCAnalysis()
{

       double reject_threshold[mpi_ranks];

	int i;

        if (mpi_rank==0) {
    BuildTestData();
	PrintTestData();
	}
        MPI::COMM_WORLD.Bcast(&TestData,ciNoOfTestDatasets*ciNoOfParameters,MPI::DOUBLE,0);
    if (bDoPureRejection==1)
        iNumberOfGenerationsForSMCAnalysis=1;
    for (i=0;i<iNumberOfGenerationsForSMCAnalysis;i++)
    {
        DoThisGenerationsRejection(i); //this figures out our new parameter values
        UpdateParameterValuesAndWeights(); //hold the old parameter values
        if (mpi_rank==0) {
                //PrintParameterValues(0);
                PrintWeightedParameterValues();
	}

        // THIS METHOD FOR REJECTION THRESHOLD TAKES THE AVERAGE OF DISTANCES BETWEEN TESTDATA AND GENERATED DATA....
	// THERE ARE OTHER WAYS TO DO THIS, BUT I PREFER THIS WAY SINCE IT MEANS ALL PROCESSES ARE AWARE OF WHEN TO QUIT
        reject_threshold[mpi_rank]=MeasureDistanceBetweenDataAndParameters();
        for (int j=0;j<mpi_ranks;j++)
                MPI::COMM_WORLD.Bcast(&reject_threshold[j],1,MPI::DOUBLE,j);
        dRejectionThreshold=0;
        for (int j=0;j<mpi_ranks;j++)
                dRejectionThreshold+=reject_threshold[j]/mpi_ranks;
	//ALL PROCESSES QUIT!
	if (dRejectionThreshold<THRESHOLD) {
	
		if (mpi_rank==0)
			cerr<<"Successfully reached dRejectionThreshold="<<dRejectionThreshold<<"<THRESHOLD="<<THRESHOLD<<endl;
		break;

	}

    }
if (mpi_rank==0) {
    //PrintParameterValues(0);
	cerr<<"Number of generations run: "<<i<<endl;
    PrintParameterValues(1);
}
}
void SMC::GenerateParameters(int iGen)
{
    double p,dProbSoFar;
    int i,j;
    if (iGen==0)
    {
        //SampledParameters[0]=SampleFromParameterPrior(0);
        //SampledParameters[1]=SampleFromParameterPrior(1);
        //SampledParameters[2]=SampleFromParameterPrior(2);

        for (i=0;i<ciNoOfParameters;i++)
                SampledParameters[i]=SampleFromParameterPrior(i);
    }
    else
    {
        for (i=0;i<ciNoOfParameters;i++)
        {

            p=gsl_rng_uniform_pos(rg);


            dProbSoFar=0;
            j=-1;

                while (dProbSoFar<p)
                 {
                     j++;
                         dProbSoFar+=dWeightsOfOldParametersValues[j];
                }

        /*if (j>=ciNoOfAcceptsNeeded)
        {
                fprintf(stderr,"j=%d, ciNoOfAcceptsNeeded=%d, p=%f\n",j,ciNoOfAcceptsNeeded,p);
        

        }*/
            assert(j<=ciNoOfAcceptsNeeded);
            SampledParameters[i]=OldAcceptedParameterValues[j][i];
            // add noise
            SampledParameters[i]+=gsl_ran_gaussian(rg,StdDev);//FIXME
        }
    }
}


void SMC::GenerateData()
{

        for (int i=0;i<ciNoOfParameters;i++)
                GeneratedData[i]=SampledParameters[i]+gsl_ran_gaussian(rg,StdDev);//NormalRV(SampledParameters[i],StdDev);
//    GeneratedData[1]=NormalRV(SampledParameters[1],StdDev);
  //  GeneratedData[2]=NormalRV(SampledParameters[2],StdDev);
}


float SMC::MeasureDistanceBetweenDataAndParameters()
{
    float d=0;
    int i,j;
    for (j=0;j<ciNoOfTestDatasets;j++)
        for (i=0; i<ciDimensionOfData;i++)
            d+=(GeneratedData[i]-TestData[j][i])*(GeneratedData[i]-TestData[j][i]);
    return d;
}

void SMC::DoThisGenerationsRejection(int iGen) //FIXME
{
    int iAccepts=0,iAttempts=0,j,NoOfAcceptsNeededPerRank=ciNoOfAcceptancesPerGeneration/mpi_ranks;
    float dDist;
double mpi_NewAcceptedParameterValues[mpi_ranks][NoOfAcceptsNeededPerRank][ciNoOfParameters];

for (iAttempts=0;iAccepts<NoOfAcceptsNeededPerRank;iAttempts++) {
        GenerateParameters(iGen);
        GenerateData();
        if ((dDist=MeasureDistanceBetweenDataAndParameters())<dRejectionThreshold) {
                for (j=0;j<ciNoOfParameters;j++)
                        mpi_NewAcceptedParameterValues[mpi_rank][iAccepts][j]=SampledParameters[j];
                        //NewAcceptedParameterValues[iAccepts+mpi_rank*NoOfAcceptsNeededPerRank][j]=SampledParameters[j];
                iAccepts++;
        }
}

MPI::COMM_WORLD.Bcast(&dWeightsOfNewParametersValues,ciNoOfAcceptsNeeded,MPI::DOUBLE,0);

for (int i=0;i<mpi_ranks;i++)
        MPI::COMM_WORLD.Bcast(&mpi_NewAcceptedParameterValues[i],NoOfAcceptsNeededPerRank*ciNoOfParameters,MPI::DOUBLE,i);

for (int i=0;i<ciNoOfAcceptsNeeded;i++)
        for (j=0;j<ciNoOfParameters;j++)
                NewAcceptedParameterValues[i][j]=mpi_NewAcceptedParameterValues[i/NoOfAcceptsNeededPerRank][i%NoOfAcceptsNeededPerRank][j];
    
    CalculateParameterWeights(iGen);
    //std::cout<<"\nGeneration "<<iGen<<" required "<<iAttempts<<" iterations";
}

void SMC::UpdateParameterValuesAndWeights()
{
    int i,j;
    for (i=0;i<ciNoOfAcceptsNeeded;i++)
    {
        dWeightsOfOldParametersValues[i]=dWeightsOfNewParametersValues[i];
        for (j=0;j<ciNoOfParameters;j++)
            OldAcceptedParameterValues[i][j]=NewAcceptedParameterValues[i][j];
    }
}

void SMC::CalculateParameterWeights(int iGen)
{
    int i,j,k;
    float dDensity,dDen,dTotalDensity,dSumOfWeights=0;
    // for each set of New Parameter values
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
    {
        if (iGen==0)
        {
            dWeightsOfNewParametersValues[k]=1.0/ciNoOfAcceptsNeeded;
            dSumOfWeights=1;
        }
        else
        {
            dTotalDensity=1;
            // for each parameter
            for (i=0;i<ciNoOfParameters;i++)
            {
                dDensity=0;
                // calculate the density of generating this value - need to go through each old parameter value to to this
                for (j=0;j<ciNoOfAcceptsNeeded;j++)
                {
 //                   dDen=gsl_ran_gaussian_pdf(NewAcceptedParameterValues[k][i]-OldAcceptedParameterValues[j][i],StdDev);
                    dDen=NormalDensity(NewAcceptedParameterValues[k][i], OldAcceptedParameterValues[j][i], StdDev);
                    dDensity+=(dDen/ciNoOfAcceptsNeeded);
                }
                dTotalDensity=dTotalDensity*dDensity;
            }
            dWeightsOfNewParametersValues[k]=dTotalDensity;
        }
    }
    
    // They now need to be inverted to recognize the importance sampling correction
    if (iGen>0)
        for (k=0;k<ciNoOfAcceptsNeeded;k++)
        {
            dWeightsOfNewParametersValues[k]=1.0/dWeightsOfNewParametersValues[k];
            dSumOfWeights+=dWeightsOfNewParametersValues[k];
        }
    
    
    // Now normalize them go become probs ready for sampling in the next generation
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
        dWeightsOfNewParametersValues[k]=dWeightsOfNewParametersValues[k]/dSumOfWeights;    
}

void SMC::InitializeArrays()
{
    for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
        dWeightsOfNewParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        dWeightsOfOldParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        for ( int j=0; j< ciNoOfParameters; j++) {
            OldAcceptedParameterValues[i][j] = -9;
            NewAcceptedParameterValues[i][j]=-9;
        }
    }
}

float SMC::SampleFromParameterPrior(int iWhichParameter)
{
    // we assume a uniform prior for now
    float dVal;
    dVal=gsl_rng_uniform(rg)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    //dVal=rand01(&ranseed)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    return dVal;
}

